#include "stdafx.h"
#include"stdio.h"
#include"VideoCapture.h"
#include <vector>



vector<CvPoint> findBlobs (IplImage *src) {
		IplImage* temp = cvCloneImage(src);

        CvMemStorage *mem_storage = cvCreateMemStorage(0);
        CvSeq* contours = NULL;

                // actually find the contours
        CvContourScanner scanner = cvStartFindContours(src, mem_storage, \
                sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

                // filter/approximate the blobs
        CvSeq* c;
        int numCont = 0;
        while( (c = cvFindNextContour( scanner )) != NULL ) {
			    // polynomial approximation
                        CvSeq* c_new;

                        c_new = cvApproxPoly(c, sizeof(CvContour), mem_storage,
                                CV_POLY_APPROX_DP, 1, 0);

                        cvSubstituteContour( scanner, c_new );
                        numCont++;
        }
        contours = cvEndFindContours( &scanner );

                // find the blob centers
        int i;
        vector<CvPoint> centers;
        CvMoments moments;
        double M00, M01, M10;

        for(i=0, c=contours; c != NULL; c = c->h_next, i++) {
                cvContourMoments(c, &moments);

                M00 = cvGetSpatialMoment(&moments,0,0);
                M10 = cvGetSpatialMoment(&moments,1,0);
                M01 = cvGetSpatialMoment(&moments,0,1);

                	// put in the output array
                centers.push_back(cvPoint((int)(M10/M00) + cvGetSize(src).width,
                        (int)(M01/M00) + cvGetSize(src).height));
        }

        	// release stuff
        cvReleaseMemStorage(&mem_storage);

        cvReleaseImage(&temp);

        return centers;
	}


void filterColor(const IplImage* src, CvScalar low, CvScalar high, IplImage** toContur, IplImage** toDisplay){
		//create destination image
	*toContur = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, src->nChannels);
	*toDisplay = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, src->nChannels);
		//create filter mask
	CvMat *mask = cvCreateMat(src->height,src->width,CV_8U);
		//put relevant pixels in mask
	cvInRangeS(src, low, high , mask);
		//set mask on newImg to black
	cvSet(*toContur, cvScalar(255,255,255), mask);
	cvSet(*toDisplay, cvScalar(0,0,0), mask);
	cvNot(mask, mask);
		//set invMask on newImg to white
	cvSet(*toContur, cvScalar(0,0,0), mask);
	cvSet(*toDisplay, cvScalar(255,255,255), mask);
	cvReleaseMat(&mask);
}

int main() {
		//Setups the camera and the main pictures to show.
	cvNamedWindow("Device 0");
	VideoCapture capt(0,320,240);
	IplImage * image = capt.CreateCaptureImage();
	IplImage *res;
	IplImage *disp;

		//Starts working until the character q is pressed.
	char c=0;
	while(c!='q') {
			//Filters the red color.
		capt.waitFrame(image);
		filterColor(image,cvScalar(0,0,100),cvScalar(100,100,255),&res,&disp);

			//res is the image where black is the not red and white is red
			// disp is the inverse of it
		cvShowImage("Device 0",image);
		cvShowImage("res",res);
		cvShowImage("disp",disp);
		
			//Counts the white dots in the white picture.
		vector<CvPoint> points = findBlobs(disp);
		printf("%d", points.size());
		c=cvWaitKey(1);
	}
	
	capt.stop();
	cvReleaseImage(&image);
	cvDestroyWindow("Device 0");

	return 0;
}
	